// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "multi_object_tracker/tracker/model/unknown_tracker.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include <bits/stdc++.h>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace perception
{
    namespace algorithm
    {
        UnknownTracker::UnknownTracker(
            const double &time, const LidarObject &object)
            : Tracker(time, object),
              z_(object.z)
        {
            object_ = object;

            // initialize params
            // measurement noise covariance
            constexpr double r_stddev_x = 1.0; // [m]
            constexpr double r_stddev_y = 1.0; // [m]
            ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
            ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);

            // Set motion model parameters
            {
                constexpr double q_stddev_x = 0.5;        // [m/s]
                constexpr double q_stddev_y = 0.5;        // [m/s]
                constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)]
                constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)]
                motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy);
            }

            // Set motion limits
            double limit_speed = 60 / 3.6;
            motion_model_.setMotionLimits(
                limit_speed, /* [m/s] maximum velocity, x */
                limit_speed  /* [m/s] maximum velocity, y */
            );

            // Set initial state
            {
                const double x = object.x;
                const double y = object.y;
                std::array<double, 36> pose_cov;
                std::array<double, 36> twist_cov;
                const double yaw = object.heading;

                double vx = 0.0;
                double vy = 0.0;

                constexpr double p0_stddev_x = 1.0; // [m]
                constexpr double p0_stddev_y = 1.0; // [m]

                const double p0_cov_x = std::pow(p0_stddev_x, 2.0);
                const double p0_cov_y = std::pow(p0_stddev_y, 2.0);

                const double cos_yaw = std::cos(yaw);
                const double sin_yaw = std::sin(yaw);
                const double sin_2yaw = std::sin(2.0 * yaw);
                pose_cov[MSG_COV_IDX::X_X] =
                    p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw;
                pose_cov[MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw;
                pose_cov[MSG_COV_IDX::Y_Y] =
                    p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw;
                pose_cov[MSG_COV_IDX::Y_X] = pose_cov[MSG_COV_IDX::X_Y];

                constexpr double p0_stddev_vx = 10 / 3.6; // [m/s]
                constexpr double p0_stddev_vy = 10 / 3.6; // [m/s]
                const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
                const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0);
                twist_cov[MSG_COV_IDX::X_X] = p0_cov_vx;
                twist_cov[MSG_COV_IDX::X_Y] = 0.0;
                twist_cov[MSG_COV_IDX::Y_X] = 0.0;
                twist_cov[MSG_COV_IDX::Y_Y] = p0_cov_vy;

                // rotate twist covariance matrix, since it is in the vehicle coordinate system
                Eigen::MatrixXd twist_cov_rotate(2, 2);
                twist_cov_rotate(0, 0) = twist_cov[MSG_COV_IDX::X_X];
                twist_cov_rotate(0, 1) = twist_cov[MSG_COV_IDX::X_Y];
                twist_cov_rotate(1, 0) = twist_cov[MSG_COV_IDX::Y_X];
                twist_cov_rotate(1, 1) = twist_cov[MSG_COV_IDX::Y_Y];
                Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix();
                Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose();
                twist_cov[MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0);
                twist_cov[MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1);
                twist_cov[MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0);
                twist_cov[MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1);

                // initialize motion model
                motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov);
            }
        }

        bool UnknownTracker::predict(const double &time)
        {
            return motion_model_.predictState(time);
        }

        LidarObject UnknownTracker::getUpdatingObject(
            const LidarObject &object)
        {
            LidarObject updating_object = object;

            // UNCERTAINTY MODEL

            // if (!object.kinematics.has_position_covariance)
            // {
            const double &r_cov_x = ekf_params_.r_cov_x;
            const double &r_cov_y = ekf_params_.r_cov_y;
            std::array<double, 36> pose_cov;
            const double pose_yaw = object.heading;
            const double cos_yaw = std::cos(pose_yaw);
            const double sin_yaw = std::sin(pose_yaw);
            const double sin_2yaw = std::sin(2.0f * pose_yaw);
            pose_cov[MSG_COV_IDX::X_X] =
                r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw;      // x - x
            pose_cov[MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y
            pose_cov[MSG_COV_IDX::Y_Y] =
                r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y
            pose_cov[MSG_COV_IDX::Y_X] = pose_cov[MSG_COV_IDX::X_Y];       // y - x
            updating_object.pose_cov = pose_cov;
            // }
            return updating_object;
        }

        bool UnknownTracker::measureWithPose(
            const LidarObject &object)
        {
            // update motion model
            bool is_updated = false;
            {
                const double x = object.x;
                const double y = object.y;

                is_updated =
                    motion_model_.updateStatePose(x, y, object.pose_cov);
                motion_model_.limitStates();
            }

            // position z
            constexpr double gain = 0.1;
            z_ = (1.0 - gain) * z_ + gain * object.z;

            return is_updated;
        }

        bool UnknownTracker::measure(
            const LidarObject &object, const double &time)
        {
            // keep the latest input object
            object_ = object;

            // check time gap
            const double dt = motion_model_.getDeltaTime(time);
            if (0.01 /*10msec*/ < dt)
            {
                std::cout << "UnknownTracker::measure There is a large gap between predicted time and measurement time. : " << dt << std::endl;
                // RCLCPP_WARN(
                //     logger_,
                //     "UnknownTracker::measure There is a large gap between predicted time and measurement time. "
                //     "(%f)",
                //     dt);
            }

            // update object
            const LidarObject updating_object =
                getUpdatingObject(object);
            measureWithPose(updating_object);

            return true;
        }

        bool UnknownTracker::getTrackedObject(
            const double &time, LidarObject &object) const
        {
            // object = object_;
            // object.object_id = getUUID();
            // object.classification = getClassification();

            Eigen::VectorXd pose, twist;
            pose << object.x, object.y, object.z, 0, 0, object.heading;
            twist << object.vx, object.vy, 0, 0, 0, object.angular_vz;
            auto pose_cov = object.pose_cov;
            auto twist_cov = object.twist_cov;

            // predict from motion model
            if (!motion_model_.getPredictedState(
                    time, pose, pose_cov, twist,
                    twist_cov))
            {
                std::cout << "UnknownTracker::getTrackedObject: Failed to get predicted state.\n";
                // RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state.");
                return false;
            }

            // position
            object.x = pose[0];
            object.y = pose[1];
            object.z = pose[2];
            object.heading = pose[6];
            object.vx = twist[0];
            object.vy = twist[1];
            object.pose_cov = pose_cov;
            object.twist_cov = twist_cov;

            object.z = z_;

            return true;
        }
    }
}